#include "force.h"
#include <iostream>
#include <fstream>
#include <cmath>

using namespace std;

force::force(neighbor *nei)
{
	neigh = nei;
	atom = nei->atom;

	N = atom->N;
	Ngroup = atom->Ngroup;
	Nm = atom->Nm;
	group = atom->group;

	N_s = atom->N_s;				
	Ngroup_s = atom->Ngroup_s;	
	Nm_s = atom->Nm_s;	

	N_p = atom->N_p;
	Ngroup_p = atom->Ngroup_p;
	Nm_p = atom->Nm_p;

	Nbond = atom->Nbond;
	Bond_ends = atom->Bond_ends;
	Bond_length = atom->Bond_length;
	Nstart = atom->Nstart;

	boxx = atom->boxx;
	boxy = atom->boxy;
	boxz = atom->boxz;

	x = atom->x;
	xcen = atom->xcen;
	f = atom->f;
	fcen = atom->fcen;

	nnabup = neigh->nnabup;
	nnabdn = neigh->nnabdn;
	upnab = neigh->upnab;
	dnnab = neigh->dnnab;

	pe0 = 1.0 / atom->rcut;
	pe0 = pe0 * pe0 * pe0 * pe0 * pe0 * pe0;
	pe0 = pe0 * pe0 - pe0;
	//pe0 = 0.0;
  
	fcoarse = new double**[Ngroup];
	for(unsigned i = 0; i < Ngroup; ++i)
	{
		fcoarse[i] = new double *[Ngroup];
		for(unsigned j = 0; j < Ngroup; ++j)
			fcoarse[i][j] = new double[3];
	}
  
	pecoarse = new double*[Ngroup];
	for(unsigned i = 0; i < Ngroup; ++i)
		pecoarse[i] = new double[Ngroup];

	frandom = new double *[Ngroup];
	frandom0 = new double *[Ngroup];
	frandom00 = new double *[Ngroup];
	for(unsigned i = 0; i < Ngroup; ++i)
	{
		frandom[i] = new double[3];
		frandom0[i] = new double[3];
		frandom00[i] = new double[3];
	}
  
	fpairrandom = new double**[Ngroup];
	fpairrandom0 = new double**[Ngroup];
	for(unsigned i = 0; i < Ngroup; ++i)
	{
		fpairrandom[i] = new double*[Ngroup];
		fpairrandom0[i] = new double*[Ngroup];
	}
	
	for(unsigned i = 0; i < Ngroup; ++i)
	for(unsigned j = 0; j < Ngroup; ++j)
	{
		fpairrandom[i][j] = new double[3];
		fpairrandom0[i][j] = new double[3];
	}
  
	rindex = new int *[Ngroup];
	rindex0 = new int *[Ngroup];
	for(unsigned i = 0; i < Ngroup; ++i)
	{
		rindex[i] = new int[Ngroup];
		rindex0[i] = new int[Ngroup];
	}
  
	for(unsigned i = 0; i < 1600; ++i)
	{
		correlation[i][0] = 0;
		correlation[i][1] = 0;
	}
}

force::~force()
{
	if(atom) delete atom;
	if(neigh) delete neigh;
	delete fcoarse;
	delete pecoarse;
	delete frand;
	delete Forcerand;
}

void force::pairforce()
{
	double rsq;
	double xij, yij, zij;
	double ftemp;
	double sr2, sr6;
	double pe = 0;

	for(unsigned i = 0; i < N; ++i)
	for(unsigned j = 0; j < 3; ++j)
		f[i][j] = 0.0;
  
	for(unsigned i = 0; i < N; ++i)
	for(int n = 0; n < nnabup[i]; ++n)
	{
		unsigned j = upnab[i][n];
		rsq = atom->dis_sq(i,j);
		sr2 = 1.0 / rsq;
		sr6 = sr2 * sr2 * sr2;

/*		if(sr6>1e6) 
		{
			std::cout<<"atom_>pe=\t"<<i<<"\t"<<j<<"\n";
			std::cout<<x[i][0]<<"\t"<<x[i][1]<<"\t"<<x[j][0]<<"\t"<<x[j][1]<<"\n";
		}	
*/
		pe += sr6*sr6 - sr6 - pe0;
			
		ftemp = sr2 * (sr6*sr6 - 0.5 * sr6);

		xij = x[i][0] - x[j][0];
		yij = x[i][1] - x[j][1];
		zij = x[i][2] - x[j][2];

		xij = xij - nint(xij/boxx) * boxx;
		yij = yij - nint(yij/boxy) * boxy;
		zij = zij - nint(zij/boxz) * boxz;

		f[i][0] += xij * ftemp;
		f[i][1] += yij * ftemp;
		f[i][2] += zij * ftemp;

		f[j][0] -= xij * ftemp;
		f[j][1] -= yij * ftemp;
		f[j][2] -= zij * ftemp;
	}

	pe = pe * 4.0;
	atom->pe = pe;

	for(unsigned i = 0; i < N; ++i)
	for(unsigned j = 0; j < 3; ++j)
		f[i][j] = f[i][j] * 48.0;

	Bond_force();

	unsigned k;
	for(unsigned i = 0; i < Ngroup; ++i)
	{
		for(unsigned l = 0; l < 3; ++l)
			fcen[i][l] = 0.0;
		
		for(unsigned j = 0; j < Nm[i]; ++j)
		{
			k = group[i][j];
			for(unsigned l = 0; l < 3; ++l)
				fcen[i][l] += f[k][l];
		}
    }
}

void force::relax_force()
{
	double rsq;
	double xij, yij, zij;
	double ftemp;
	double Factor = 1000.0;

	for(unsigned i = 0; i < N; ++i)
	for(unsigned j = 0; j < 3; ++j)
		f[i][j] = 0.0;
  
	for(unsigned i = 0; i < N; ++i)
	for(int n = 0; n < nnabup[i]; ++n)
	{
		unsigned j = upnab[i][n];
		rsq = atom->dis_sq(i,j);

		if(rsq <1.0)
			ftemp = Factor*(1-rsq);
		else
			ftemp = 0;

		xij = x[i][0] - x[j][0];
		yij = x[i][1] - x[j][1];
		zij = x[i][2] - x[j][2];

		xij = xij - nint(xij/boxx) * boxx;
		yij = yij - nint(yij/boxy) * boxy;
		zij = zij - nint(zij/boxz) * boxz;

		f[i][0] += xij * ftemp;
		f[i][1] += yij * ftemp;
		f[i][2] += zij * ftemp;

		f[j][0] -= xij * ftemp;
		f[j][1] -= yij * ftemp;
		f[j][2] -= zij * ftemp;
	}

	Bond_force();

	unsigned k;
	for(unsigned i = 0; i < Ngroup; ++i)
	{
		for(unsigned l = 0; l < 3; ++l)
			fcen[i][l] = 0.0;
		
		for(unsigned j = 0; j < Nm[i]; ++j)
		{
			k = group[i][j];
			for(unsigned l = 0; l < 3; ++l)
				fcen[i][l] += f[k][l];
		}
    }
}

void force::Bond_force()
{
	double rsq, r1;
	double xij, yij, zij;
	double ftemp;
	double Kb=20;

	for(unsigned n = 0; n < Nbond; ++n)
	{
		unsigned i = Bond_ends[n][0];
		unsigned j = Bond_ends[n][1];
		double L0=Bond_length[n];

		rsq = atom->dis_sq(i,j);
		r1 = std::sqrt(rsq);

		ftemp=Kb*(L0*L0-rsq)/r1;

		xij = x[i][0] - x[j][0];
		yij = x[i][1] - x[j][1];
		zij = x[i][2] - x[j][2];

		xij = xij - nint(xij/boxx) * boxx;
		yij = yij - nint(yij/boxy) * boxy;
		zij = zij - nint(zij/boxz) * boxz;

		f[i][0] += xij * ftemp;
		f[i][1] += yij * ftemp;
		f[i][2] += zij * ftemp;

		f[j][0] -= xij * ftemp;
		f[j][1] -= yij * ftemp;
		f[j][2] -= zij * ftemp;
	}

//	std::cout<<"Bond_force have been calculated.\n";
}


void force::statforce_init()
{
    
	numstat = new int[fsoftbin];
	fstat = new double[fsoftbin];
	pestat = new int*[fsoftbin];
	fnumstat = new double*[fsoftbin];
  
	for(int i = 0; i < fsoftbin; ++i)
	{
		fstat[i] = 0;
		numstat[i] = 0;
		pestat[i] = new int[80];
		fnumstat[i] = new double[80];
	}
  
	for(int i = 0; i < fsoftbin; ++i)
	{
		for(unsigned j = 0; j < 80; ++j)
		{
			pestat[i][j] = 0;
			fnumstat[i][j] = 0;
		}
	}

  
	frand = new int*[fsoftbin];
	for(int i = 0; i < fsoftbin; ++i)
		frand[i] = new int[fbin];

	for(int i = 0; i < fsoftbin; ++i)
	for(int j = 0; j < fbin; ++j)
		frand[i][j] = 0;

	frcorrelation = new double**[fsoftbin];
	for(int i = 0; i < fsoftbin; ++i)
		frcorrelation[i] = new double*[2000];

	for(int i = 0; i < fsoftbin; ++i)
	for(int j = 0; j < 2000; ++j)
		frcorrelation[i][j] = new double[4];

	for(int i = 0; i < fsoftbin; ++i)
	for(int j = 0; j < 2000; ++j)
	for(int k = 0; k < 4; ++k)
		frcorrelation[i][j][k] = 0;

	Forcerand = new double*[Fbin];
	for(int i = 0; i < Fbin; ++i)
		Forcerand[i] = new double[3];
  
  
	fave = new double[fsoftbin];
	fspan = new double[fsoftbin];
  
	ifstream force_read("force_ave.dat");
	double temp1, temp2, temp3, temp4;
	for(int i = 0; i < fsoftbin; ++i)
	{
		if(!force_read)
		{
			temp1=0;	 temp2=0;	temp3=0;	temp4=0;
//			cout << "open force_ave.dat failed ! But it doesn't matter.\t @force.cpp L275.\n";
		}
		else
			force_read>>temp1>>temp2>>temp3>>temp4;
	
		if(temp4 != 0)
			fave[i] = temp3 / temp4;
		else
			fave[i] = 0;
		
//		std::cout<<(i+0.5)*fsoftspan/fsoftbin<<" "<<fave[i]<<"\n";
	}
  
	for(int i = fsoftbin - 2; i >= 0; --i)
	if(fave[i] == 0)
		fave[i] = fave[i+1];
  
	for(int i = fsoftbin -1; i >= 6; --i)
	{
		if(fabs(fave[i]) < 0.2)
			fspan[i] = 5.0;
		else if(fabs(fave[i]) < 3.0)
			fspan[i] = 100;
		else if(fabs(fave[i]) < 20.0)
			fspan[i] = 400;
		else
			fspan[i] = 500.0;
	}
	
	for(int i = 0; i < 6; ++i)
		fspan[i] = 500.0;
 
  force_read.close();
}


double average_force(double r, double* fave, double dr, int Nbin)
{
	double ff;
	int it = static_cast<int>(r/dr + 0.5);
	
	if(it == 0)
		ff = fave[0];
	else if(it >= Nbin)
		ff = 0;
	else
		ff = (r - (it - 0.5)*dr) * (fave[it] - fave[it-1]) / dr + fave[it-1];

	return ff;
}

/*
void force::statforce(unsigned & loop)
{
  double rsq;
  double xij, yij, zij;
  double ftemp;
  double sr2, sr6;
  double pe = 0;
  double pe_temp;
  
  unsigned igroup, jgroup;
  
  for(unsigned i = 0; i < N; ++i)
    for(unsigned j = 0; j < 3; ++j)
      f[i][j] = 0.0;

  for(unsigned i = 0; i < Ngroup; ++i)
    for(unsigned j = 0; j < Ngroup; ++j)
      {
	pecoarse[i][j] = 0;
	for(unsigned k = 0; k < 3; ++k)
	  fcoarse[i][j][k] = 0.0;
      }

  for(unsigned i = 0; i < Ngroup; ++i)
    for(unsigned j = 0; j < 3; ++j)
      frandom[i][j] = 0;
  
  for(unsigned i = 0; i < N; ++i)
    {
      igroup = i / Nm;
      for(unsigned n = 0; n < nnabup[i]; ++n)
	{
	  unsigned j = upnab[i][n];
	  jgroup = j/Nm;
	  rsq = atom->dis_sq(i,j);
	  sr2 = 1.0 / rsq;
	  sr6 = sr2 * sr2 * sr2;
	  pe_temp = sr6*sr6 - sr6 - pe0;
	  pe += pe_temp;
	  ftemp = sr2 * (sr6*sr6 - 0.5 * sr6);
	  
	  xij = x[i][0] - x[j][0];
	  yij = x[i][1] - x[j][1];
	  zij = x[i][2] - x[j][2];
	  
	  xij = xij - nint(xij/boxx) * boxx;
	  yij = yij - nint(yij/boxy) * boxy;
	  zij = zij - nint(zij/boxz) * boxz;
	  
	  f[i][0] += xij * ftemp;
	  f[i][1] += yij * ftemp;
	  f[i][2] += zij * ftemp;
	  
	  f[j][0] -= xij * ftemp;
	  f[j][1] -= yij * ftemp;
	  f[j][2] -= zij * ftemp;
	        
	  if(igroup != jgroup)
	    {
	      if(igroup > jgroup)
		std::cout<<"strange!!!\n";
	      fcoarse[igroup][jgroup][0] += xij * ftemp;
	      fcoarse[igroup][jgroup][1] += yij * ftemp;
	      fcoarse[igroup][jgroup][2] += zij * ftemp;
	      pecoarse[igroup][jgroup] += 4.0 * pe_temp;
	    }
	}
    }
  
  pe = pe * 4.0;
  atom->pe = pe;
  
  for(unsigned i = 0; i < N; ++i)
    for(unsigned j = 0; j < 3; ++j)
      f[i][j] = f[i][j] * 48.0;
  unsigned k;
  
  for(unsigned i = 0; i < Ngroup; ++i)
    {
      for(unsigned l = 0; l < 3; ++l)
        fcen[i][l] = 0.0;
      for(unsigned j = 0; j < Nm; ++j)
        {
          k = group[i][j];
          for(unsigned l = 0; l < 3; ++l)
            fcen[i][l] += f[k][l];
        }
    }
  exc_pressure = 0;
  for(unsigned i = 0; i < Ngroup - 1; ++i)
    for(unsigned j = i + 1; j < Ngroup; ++j)
      {
	double xijcen = xcen[i][0] - xcen[j][0];
	double yijcen = xcen[i][1] - xcen[j][1];
	double zijcen = xcen[i][2] - xcen[j][2];
	xijcen = xijcen - nint(xijcen/boxx) * boxx;
	yijcen = yijcen - nint(yijcen/boxy) * boxy;
	zijcen = zijcen - nint(zijcen/boxz) * boxz;
	
	double rcen = xijcen * xijcen + yijcen * yijcen + zijcen * zijcen;
	rcen = sqrt(rcen);
	
	int index = static_cast<int>(rcen * fsoftbin / fsoftspan);
	
	
	
	if (index >= fsoftbin)
	  index = fsoftbin - 1 ;
	
	rindex[i][j] = index;
	
	double petemp = pecoarse[i][j];
	int peindex = static_cast<int>((petemp + 15)*2.0);
    
	if(peindex < 0)
	  peindex = 0;
	if(peindex >= 80)
	  peindex = 79;
	
	double ftemp = 48.0 * (fcoarse[i][j][0] * xijcen + fcoarse[i][j][1] * yijcen + 
				fcoarse[i][j][2] * zijcen);
	exc_pressure += ftemp;
	
	ftemp = ftemp / rcen;
	
	
	fstat[index] += ftemp;
	numstat[index] += 1;
	
	pestat[index][peindex] += 1;
	fnumstat[index][peindex] += ftemp;
	
	
	double faverage = average_force(rcen, fave, fsoftspan/fsoftbin, fsoftbin);
	
	double fdiff = ftemp - faverage;
	fpairrandom[i][j][0] = (fdiff) * xijcen / rcen;
	fpairrandom[i][j][1] = (fdiff) * yijcen / rcen;
	fpairrandom[i][j][2] = (fdiff) * zijcen / rcen;
	
	
	frandom[i][0] += fpairrandom[i][j][0];
	frandom[i][1] += fpairrandom[i][j][1];
	frandom[i][2] += fpairrandom[i][j][2];
	
	frandom[j][0] -= fpairrandom[i][j][0];
	frandom[j][1] -= fpairrandom[i][j][1];
	frandom[j][2] -= fpairrandom[i][j][2];
      }
  
  

  if(loop % 5000 == 0)
    {
      for(unsigned i = 0; i < Ngroup - 1; ++i)
	for(unsigned j = i + 1; j < Ngroup; ++j)
	  for(int k = 0; k < 3; ++k)
	    fpairrandom0[i][j][k] = fpairrandom[i][j][k];
      
      for(int i = 0; i < Ngroup - 1; ++i)
	for(int j = i + 1; j < Ngroup; ++j)
	  rindex0[i][j] = rindex[i][j];
    }
  
  int it = loop % 5000;
  it = it / 5;
 /* 
  for(int i = 0; i < Ngroup - 1; ++i)
    for(int j = i+1; j < Ngroup; ++j)
      {
	
	int index = rindex[i][j];
	if ((index < fsoftbin)&&(index == rindex0[i][j]))
	  {
	    for(int k = 0; k < 3; ++k)
	    frcorrelation[index][it][0] += fpairrandom0[i][j][k] * fpairrandom[i][j][k];
	    frcorrelation[index][it][1] += 1.0;
	    }
	

	int index = rindex0[i][j];
	for(int k = 0; k < 3; ++k)
	  frcorrelation[index][it][0] += fpairrandom0[i][j][k] * fpairrandom[i][j][k];                                                                                                           
	frcorrelation[index][it][1] += 1.0;  
      }
  *//*
  
  if(loop % 5000 == 0)
    {
      for(unsigned i = 0; i < Ngroup; ++i)
	for(unsigned j = 0; j < 3; ++j)
	  frandom0[i][j] = frandom[i][j];
    }
  
  double corr = 0;
  double corr1 = 0;
  for(unsigned i = 0; i < Ngroup; ++i)
    for(unsigned k = 0; k < 3; ++k)
      corr += frandom[i][k] * frandom0[i][k];
  
   
  for(unsigned i = 0; i < Ngroup; ++i)
    for(unsigned j = 0; j < Ngroup; ++j)
      for(unsigned k = 0; k < 3; ++k)
	corr1 += frandom[i][k] * frandom0[j][k];
  corr1 = corr1 - corr;
  
  
  it = loop % 5000;
  it = it / 5;
  int number = loop / 5000 + 1;
  correlation[it][0] += corr / Ngroup;
  correlation[it][1] += corr1 / Ngroup;
  correlation[it][2] = correlation[it][0] / number;
  correlation[it][3] = correlation[it][1] / number;
  */
  /*
  for(unsigned i = 0; i < Ngroup; ++i)
    for(unsigned j = 0; j < 3; ++j)
      {
	int findex = static_cast<int>((frandom[i][j] + 0.5 * Fspan) * Fbin / Fspan);
	if(findex < 0)
	  findex = 0;
	if(findex >= Fbin)
	  findex = Fbin - 1;
	
	Forcerand[findex][j] += 1.0;
      }
  */

  /*
  
}*/
